#include "tcp_manager.h"
#include <sys/socket.h>
#include <arpa/inet.h>
#include <unistd.h>
#include <iostream>
#include <cstring>
#include <chrono>
#include <sys/sendfile.h> 
#include <fcntl.h>        
#include <sys/mman.h>     
#include <sys/stat.h>     
static CBuff s_NetBuffer;
static uint8_t sa_NetBuffer_Space[4096];
static bool SendThereadSwitch_ = false;
int SharedMem_Recvfd;
int SharedMem_Sendfd;
SharedMemory_Recv_TypeDef *NetSharedMemoryRecv;
SharedMemory_Send_TypeDef *NetSharedMemorySend;
Trace_SendData_Typedef s_SendData;
Trace_SendFlag_Typedef s_SendFlag;

void NetSharedMemInit()
{
    SharedMem_Recvfd = shm_open(SHM_NAME_RECV, O_CREAT | O_RDWR, 0666);
    if (SharedMem_Recvfd == -1) {
        std::cerr << "Error: Unable to open shared memory for writing." << std::endl;
        return;
    }
    ftruncate(SharedMem_Recvfd, sizeof(SharedMemory_Recv_TypeDef));
    NetSharedMemoryRecv = (SharedMemory_Recv_TypeDef *)mmap(nullptr, sizeof(SharedMemory_Recv_TypeDef), PROT_WRITE, MAP_SHARED, SharedMem_Recvfd, 0);
    if (NetSharedMemoryRecv == MAP_FAILED) {
        std::cerr << "Error: Unable to map shared memory for writing." << std::endl;
        close(SharedMem_Recvfd);
        return;
    }

    std::cout << "Shared memory address in app: " << NetSharedMemoryRecv << std::endl;

    if(sem_init(&NetSharedMemoryRecv->Sem_Notice, 1, 0) == -1)
    {
        std::cerr << "Error: Unable to init semaphore." << std::endl;
    }
    NetSharedMemoryRecv->DataState = DataState_Idle;

    SharedMem_Sendfd = shm_open(SHM_NAME_SEND, O_CREAT | O_RDWR, 0666);
    if (SharedMem_Sendfd == -1) {
        std::cerr << "Error: Unable to open shared memory for writing." << std::endl;
        return;
    }
    ftruncate(SharedMem_Sendfd, sizeof(SharedMemory_Send_TypeDef));
    NetSharedMemorySend = (SharedMemory_Send_TypeDef *)mmap(nullptr, sizeof(SharedMemory_Send_TypeDef), PROT_WRITE, MAP_SHARED, SharedMem_Sendfd, 0);
    if (NetSharedMemorySend == MAP_FAILED) {
        std::cerr << "Error: Unable to map shared memory for writing." << std::endl;
        close(SharedMem_Sendfd);
        return;
    }
    if(sem_init(&NetSharedMemorySend->Sem_Notice, 1, 0) == -1)
    {
        std::cerr << "Error: Unable to init semaphore." << std::endl;   
    }
}

void NetSharedMemunInit()
{
    munmap(NetSharedMemoryRecv, sizeof(SharedMemory_Recv_TypeDef));
    munmap(NetSharedMemorySend, sizeof(SharedMemory_Send_TypeDef));
    close(SharedMem_Recvfd);
    close(SharedMem_Sendfd);
    shm_unlink(SHM_NAME_RECV);
    shm_unlink(SHM_NAME_SEND);
}

TCPManager::TCPManager() {
    // 初始化日志系统
    std::cout << "TCPManager has been started." << std::endl;
}

TCPManager::~TCPManager() {
    NetSharedMemunInit();
    shutdown();
}

TCPManager& TCPManager::getInstance() {
    static TCPManager instance;
    return instance;
}

bool TCPManager::init(const char* ip, int port) {
    serverIp_ = ip;
    serverPort_ = port;

    if (!reconnect()) {
        return false;
    }
    NetSharedMemInit();
    SendThereadSwitch_ = true;
    running_ = true;
    sendThread_ = std::thread(&TCPManager::sendThread, this);
    receiveThread_ = std::thread(&TCPManager::receiveThread, this);
    std::cout << "TCP connection initialized to " << serverIp_ << ":" << serverPort_ << std::endl;
    return true;
}

void TCPManager::shutdown() {
    running_ = false;
    queueCond_.notify_all();
    if (sendThread_.joinable()) {
        sendThread_.join();
    }
    if (receiveThread_.joinable()) {
        receiveThread_.join();
    }
    if (sockfd_ != -1) {
        close(sockfd_);
        sockfd_ = -1;
    }
    std::cout << "TCP connection shutdown" << std::endl;
}

void TCPManager::registerErrorCallback(ErrorCallback cb) {
    std::lock_guard<std::mutex> lock(callbackMutex_);
    errorCallback_ = std::move(cb);
}

bool TCPManager::sendImmediate(const uint8_t* data, size_t len) {
    std::lock_guard<std::mutex> lock(queueMutex_);
    return rawSend(data, len);
}

void TCPManager::sendBuffered(const uint8_t* data, size_t len, Priority priority) {
    {
        std::lock_guard<std::mutex> lock(queueMutex_);
        if (bufferQueues_[priority].size() < 1000) { // limit queue size
            bufferQueues_[priority].emplace(encodePacket(data, len));
        } else {
            notifyError("Queue overflow for priority " + std::to_string(static_cast<int>(priority)));
            return;
        }
    }
    queueCond_.notify_one();
}

bool TCPManager::sendFile(const std::string& filePath, size_t chunkSize) {
    int fileFd = open(filePath.c_str(), O_RDONLY);
    if (fileFd < 0) {
        std::cout << "Failed to open file: " << filePath << std::endl;
        return false;
    }

    struct stat fileStat;
    if (fstat(fileFd, &fileStat)) {
        std::cout << "Failed to get file size: " << filePath << std::endl;
        close(fileFd);
        return false;
    }

    size_t fileSize = fileStat.st_size;
    void* fileData = mmap(nullptr, fileSize, PROT_READ, MAP_PRIVATE, fileFd, 0);
    if (fileData == MAP_FAILED) {
        std::cout << "Failed to mmap file: " << filePath << std::endl;
        close(fileFd);
        return false;
    }

    size_t offset = 0;
    while (offset < fileSize) {
        size_t remaining = fileSize - offset;
        size_t sendSize = (remaining < chunkSize) ? remaining : chunkSize;

        ssize_t sent = send(sockfd_, static_cast<uint8_t*>(fileData) + offset, sendSize, MSG_NOSIGNAL);
        if (sent < 0) {
            std::cout << "Failed to send file chunk: " << filePath << std::endl;
            munmap(fileData, fileSize);
            close(fileFd);
            return false;
        }

        offset += sent;
        std::cout << "Sent " << offset << " bytes of file " << filePath << std::endl; 
    }

    munmap(fileData, fileSize);
    close(fileFd);
    std::cout << "File sent successfully: " << filePath << ", size: " << fileSize << std::endl;
    return true;
}

TCPManager::PerformanceStats TCPManager::getPerformanceStats() const {
    std::lock_guard<std::mutex> lock(statsMutex_);
    PerformanceStats stats;
    stats.totalSent = totalSent_;
    stats.totalFailed = totalFailed_;
    stats.queueSize = 0;
    stats.totalReceived = totalReceived_;
    for (const auto& [_, queue] : bufferQueues_) {
        stats.queueSize += queue.size();
    }
    return stats;
}

void TCPManager::sendThread() {
    while (running_) {
        std::unique_lock<std::mutex> lock(queueMutex_);
        queueCond_.wait(lock, [this]{
            return !bufferQueues_[Priority::HIGH].empty() ||
                   !bufferQueues_[Priority::NORMAL].empty() ||
                   !bufferQueues_[Priority::LOW].empty() ||
                   !running_;
        });

        // handle high priority first
        for (auto priority : {Priority::HIGH, Priority::NORMAL, Priority::LOW}) {
            while (!bufferQueues_[priority].empty()) {
                auto& data = bufferQueues_[priority].front();
                if (!rawSend(data.data(), data.size())) {
                    break; // stop sending if error occurs
                }
                bufferQueues_[priority].pop();
            }
        }
    }
}

void TCPManager::receiveThread() {
    uint8_t buffer[64];
    ReciveThereadSwitch_ = true;
    while (running_) {
        if(ReciveThereadSwitch_ == true )
        {
            ssize_t received = recv(sockfd_, buffer, sizeof(buffer), 0);
            if (received > 0) {
                std::lock_guard<std::mutex> lock(statsMutex_);
                totalReceived_ += received;
                //std::cout << "Received " << received << " bytes" << std::endl;
                CBuff_Write(&s_NetBuffer, buffer, received);
            } else if (received == 0) {
                std::cout << "Server closed connection" << std::endl;
                ReciveThereadSwitch_ = false;
            } else {
                std::cout << "Receive error: " << strerror(errno) << std::endl;
                ReciveThereadSwitch_ = false;
            }
        }
        else
        {
            std::this_thread::sleep_for(std::chrono::milliseconds(10)); // wait for reconnect
        }
    }
}

bool TCPManager::rawSend(const uint8_t* data, size_t len) {
    ssize_t sent = send(sockfd_, data, len, MSG_NOSIGNAL);
    if (sent < 0) {
        notifyError("Send failed: " + std::string(strerror(errno)));
        if (!reconnect()) {
            return false;
        }
    } else {
        std::lock_guard<std::mutex> lock(statsMutex_);
        totalSent_ += len;
    }
    return true;
}

bool TCPManager::reconnect() {
    const int maxRetries = 3;
    for (int i = 0; i < maxRetries; ++i) {
        if (sockfd_ != -1) {
            close(sockfd_);
        }

        sockfd_ = socket(AF_INET, SOCK_STREAM, 0);
        if (sockfd_ < 0) {
            continue;
        }

        sockaddr_in serverAddr{};
        serverAddr.sin_family = AF_INET;
        serverAddr.sin_port = htons(serverPort_);
        if (inet_pton(AF_INET, serverIp_.c_str(), &serverAddr.sin_addr) <= 0) {
            continue;
        }

        if (connect(sockfd_, (struct sockaddr*)&serverAddr, sizeof(serverAddr)) < 0) {
            std::this_thread::sleep_for(std::chrono::seconds(1)); // wait for 1 second before retry
            continue;
        }

        ReciveThereadSwitch_ = true;
        std::cout << "Reconnected to server" << std::endl;
        return true;
    }

    notifyError("Reconnect failed after " + std::to_string(maxRetries) + " attempts");
    return false;
}

void TCPManager::notifyError(const std::string& msg) {
    std::cout << "Error: " << msg << std::endl;
    std::lock_guard<std::mutex> lock(callbackMutex_);
    if (errorCallback_) {
        errorCallback_(msg);
    }
}

std::vector<uint8_t> TCPManager::encodePacket(const uint8_t* data, size_t len) {
    std::vector<uint8_t> packet;
    packet.insert(packet.end(), data, data + len);
    return packet;
}

void handleError(const std::string& error) {
    std::cerr << "Critical Error: " << error << std::endl;
}

int app_net_handle() {

    CBuff_Init(&s_NetBuffer, sa_NetBuffer_Space, sizeof(sa_NetBuffer_Space));
    auto& tcp = TCPManager::getInstance();
    tcp.registerErrorCallback(handleError);

    // 214
    if (!tcp.init("192.168.132.214", 8080)) {
        return 1;
    }
    while (true) {
        std::this_thread::sleep_for(std::chrono::seconds(10)); // keep alive
    }
    return 0;
}

void App_Net_Datahandle(uint8_t *buffer)
{
    switch(buffer[3])
    {
        case E_CMD_READVER:  // Ver
        {
            std::cout << "receive SoftWare Ver " << static_cast<int>(buffer[4]) <<"."<< static_cast<int>(buffer[5]) <<std::endl;
            std::cout << "receive HardWare Ver " << static_cast<int>(buffer[6]) <<std::endl;
            std::cout << "receive BootLoader Ver " << static_cast<int>(buffer[7]) <<std::endl;
            if(NetSharedMemoryRecv->DataState == DataState_Idle)
            {
                // copy data to shared memory
                printf("copy data to shared memory\n");
                NetSharedMemoryRecv->DataType = E_CMD_READVER;
                NetSharedMemoryRecv->DataState = DataState_Working;
                memcpy(&NetSharedMemoryRecv->Data.Ver, buffer+4, 4);
                sem_post(&NetSharedMemoryRecv->Sem_Notice);
            }
            break;
        }

        case E_CMD_HEARTBEAT:  // heartbeat
        {
            //std::cout << "receive HeartBeat " << buffer[4] << std::endl;
            break;
        }
        case E_CMD_PID_LEFT:
        {
            //std::cout << "receive left " << buffer[4] << std::endl;
            break;
        }
        case E_CMD_PID_RIGHT:
        {
            //std::cout << "receive right " << buffer[4] << std::endl;
            break;
        }
        case E_CMD_PID_STAND:
        {
            //std::cout << "receive stand " << buffer[4] << std::endl;
            break;
        }
        case E_CMD_PID_STER:
        {
            //std::cout << "receive ster " << buffer[4] << std::endl;
            break;
        }
        default:
        {
            break;
        }
    }
}

void View_Send()
{
    if(SendThereadSwitch_ == false)
    {
        return;
    }
    if(sem_trywait(&NetSharedMemorySend->Sem_Notice) == 0)
    {
        switch(NetSharedMemorySend->DataType)
        {
            case E_CMD_READVER:  // Ver
            {
                std::cout << "ros read ver send " << std::endl;
                s_SendData.Ctrl.ReadVer = NetSharedMemorySend->Data.Ctrl.ReadVer;
                NetSharedMemorySend->Data.Ctrl.ReadVer = 0;
                if(s_SendData.Ctrl.ReadVer == 1)
                {
                    s_SendData.Ctrl.ReadVer = 0;
                    s_SendFlag.ReadVer = 1;
                }
                break;
            }

            case E_CMD_HEARTBEAT:  // heartbeat
            {
                break;
            }
            case E_CMD_PID_LEFT:
            {
                break;
            }
            case E_CMD_PID_RIGHT:
            {
                break;
            }
            case E_CMD_PID_STAND:
            {
                break;
            }
            case E_CMD_PID_STER:
            {
                break;
            }
            default:
            {
                break;
            }
        }
        NetSharedMemorySend->DataState = DataState_Idle;
    }
}

void App_Net_CycleSend()
{
    static uint32_t s_Timer_Cnt;
    auto& tcp = TCPManager::getInstance();
    s_Timer_Cnt++;

    View_Send();

    if(s_SendFlag.ReadVer == 1)
    {
        uint8_t controlCmd[] = {0x02};
        uint8_t *PackedData;
        PackedData = ComService_PackageData(E_COMFRAMEHEAD_NORMAL,E_COMFRAMETAIL_CRC16,controlCmd,3);
        tcp.sendBuffered(PackedData, PackedData[PackDataLen], TCPManager::Priority::NORMAL);
        s_SendFlag.ReadVer = 0;
        return;
    }

    if(s_Timer_Cnt%200 == 0)  //5ms*200  1s
    {
        uint8_t controlCmd[] = {0x03, 0xA0, 0xFF};
        uint8_t *PackedData;
        PackedData = ComService_PackageData(E_COMFRAMEHEAD_NORMAL,E_COMFRAMETAIL_CRC16,controlCmd,3);
        tcp.sendBuffered(PackedData, PackedData[PackDataLen], TCPManager::Priority::NORMAL);     
        return;  
    }

    // 10s timer
    if(s_Timer_Cnt%2000 == 0)  //5ms*2000  10s
    {
        s_Timer_Cnt = 0;
        // get performance stats
        auto stats = tcp.getPerformanceStats();
        std::cout << "Perform:"<< "Total Sent: " << stats.totalSent << ", Total Failed: " << stats.totalFailed
                << ", Queue Size: " << stats.queueSize << ", Total Received: " << stats.totalReceived << std::endl;
    }
/**********************************************************************/
    // // 发送高优先级控制指令（直接发送）
    // uint8_t controlCmd[] = {0x01, 0xA0, 0xFF};
    // tcp.sendImmediate(controlCmd, sizeof(controlCmd));

    // // 发送普通传感器数据（缓冲发送）
    // for (int i = 0; i < 100; ++i) {
    //     uint8_t sensorData[] = {0x02, static_cast<uint8_t>(i)};
    //     tcp.sendBuffered(sensorData, sizeof(sensorData), TCPManager::Priority::NORMAL);
    //     std::this_thread::sleep_for(std::chrono::milliseconds(10));
    // }

    // // 发送文件（分块发送 + mmap）
    // tcp.sendFile("example.txt", 1024 * 1024); // 分块大小 1MB
    
}


void App_Net_5msCycle(void)
{
    static uint8_t s_NetHandle_Buffer[64];
    // recive handle
    if(ComService_UnpackData(&s_NetBuffer, s_NetHandle_Buffer) == 1)
    {
        App_Net_Datahandle(s_NetHandle_Buffer);           
    }
    // send handle 
    App_Net_CycleSend();
}